/***************************************************************************
 创建者: 华磊
 开始时间: 2018.11.6
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "amclconfig.h"
#include "domparser.h"
#include <qdebug.h>
#include <QVariant>
#include <QSettings>

AmclConfig::AmclConfig(QString configFileIn,QString positionConfigPathIn)
{
    configFile=configFileIn;
    positionConfigPath=positionConfigPathIn;

    newOdom_alpha_badMatchScore=0.005;
    badMatchScore=10;

}

int AmclConfig::readConfigFile(QString filePath, QString positionConfigPathIn)
{

    //position read

    QSettings settings(positionConfigPathIn, QSettings::IniFormat);

    QString strConfigKey;

    strConfigKey= "floorPosition/position_x";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        return -1;
    }
    initial_pose_x = settings.value(strConfigKey).toDouble();

    strConfigKey= "floorPosition/position_y";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        return -1;
    }
    initial_pose_y = settings.value(strConfigKey).toDouble();

    strConfigKey= "floorPosition/position_rotate";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,runConfig.ini loose parameter";
        return -1;
    }
    initial_pose_a = settings.value(strConfigKey).toDouble();


    //
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePath, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, AmclConfig::readConfigFile open file error！";
       return -1;
    }

    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "node", ok );
    if( !ok )
    {

        qDebug()<<"error, AmclConfig::readConfigFile node error！";
        return -1;
    }

    QDomNodeList devNodes = inNode.childNodes();
    QDomNode devND;
    QVector<RosXml> tmpList;
    RosXml tmpValue;

    for(int i=0;i<devNodes.size();i++)
    {
        devND =  devNodes.at(i);
        if(false==devND.toElement().hasAttribute("name" ))
        {
            continue;
        }
        tmpValue.name= devND.toElement().attribute("name" );

        if(false==devND.toElement().hasAttribute("value" ))
        {
            continue;
        }
        tmpValue.value= devND.toElement().attribute("value" );
        tmpList.append(tmpValue);
    }

    int tmpResult;
    QString tmpString;
    tmpResult=getValue("laser_model_type",tmpList,tmpString);
    if(1==tmpResult)
    {
        if(tmpString == "beam")
          laser_model_type = amcl::LASER_MODEL_BEAM;
        else if(tmpString == "likelihood_field")
          laser_model_type = amcl::LASER_MODEL_LIKELIHOOD_FIELD;
        else if(tmpString == "likelihood_field_prob")
          laser_model_type = amcl::LASER_MODEL_LIKELIHOOD_FIELD_PROB;
    }
    else
    {
        return -1;
    }

    tmpResult=getValue("odom_model_type",tmpList,tmpString);
    if(1==tmpResult)
    {
        if(tmpString == "diff")
          odom_model_type = amcl::ODOM_MODEL_DIFF;
        else if(tmpString == "omni")
          odom_model_type = amcl::ODOM_MODEL_OMNI;
        else if(tmpString == "diff-corrected")
          odom_model_type = amcl::ODOM_MODEL_DIFF_CORRECTED;
        else if(tmpString == "omni-corrected")
          odom_model_type = amcl::ODOM_MODEL_OMNI_CORRECTED;
    }
    else
    {
        return -2;
    }

    tmpResult=getValue("laser_max_beams",tmpList,tmpString);
    if(1==tmpResult)
    {
        laser_max_beams=tmpString.toInt();
    }
    else
    {
        return -3;
    }

    tmpResult=getValue("min_particles",tmpList,tmpString);
    if(1==tmpResult)
    {
        min_particles=tmpString.toInt();
    }
    else
    {
        return -4;
    }

    tmpResult=getValue("max_particles",tmpList,tmpString);
    if(1==tmpResult)
    {
        max_particles=tmpString.toInt();
    }
    else
    {
        return -5;
    }

    tmpResult=getValue("update_min_d",tmpList,tmpString);
    if(1==tmpResult)
    {
        update_min_d=tmpString.toDouble();
    }
    else
    {
        return -6;
    }

    tmpResult=getValue("update_min_a",tmpList,tmpString);
    if(1==tmpResult)
    {
        update_min_a=tmpString.toDouble();
    }
    else
    {
        return -7;
    }

    tmpResult=getValue("odom_alpha1",tmpList,tmpString);
    if(1==tmpResult)
    {
        odom_alpha1=tmpString.toDouble();
    }
    else
    {
        return -8;
    }

    tmpResult=getValue("odom_alpha2",tmpList,tmpString);
    if(1==tmpResult)
    {
        odom_alpha2=tmpString.toDouble();
    }
    else
    {
        return -9;
    }

    tmpResult=getValue("odom_alpha3",tmpList,tmpString);
    if(1==tmpResult)
    {
        odom_alpha3=tmpString.toDouble();
    }
    else
    {
        return -10;
    }

    tmpResult=getValue("odom_alpha4",tmpList,tmpString);
    if(1==tmpResult)
    {
        odom_alpha4=tmpString.toDouble();
    }
    else
    {
        return -11;
    }

    tmpResult=getValue("odom_alpha5",tmpList,tmpString);
    if(1==tmpResult)
    {
        odom_alpha5=tmpString.toDouble();
    }
    else
    {
        return -12;
    }

    tmpResult=getValue("isUpdatePartialEveryTime",tmpList,tmpString);
    if(1==tmpResult)
    {
        isUpdatePartialEveryTime=tmpString.toInt();
    }
    else
    {
        return -13;
    }

    tmpResult=getValue("calculateTimesForLaser",tmpList,tmpString);
    if(1==tmpResult)
    {
        calculateTimesForLaser=tmpString.toInt();
    }
    else
    {
        return -13;
    }

    tmpResult=getValue("isUseRosLaserMsg",tmpList,tmpString);
    if(1==tmpResult)
    {
        isUseRosLaserMsg=tmpString.toInt();
    }
    else
    {
        return -14;
    }

//    tmpResult=getValue("initial_pose_x",tmpList,tmpString);
//    if(1==tmpResult)
//    {
//        initial_pose_x=tmpString.toDouble();
//    }
//    else
//    {
//        return -15;
//    }

//    tmpResult=getValue("initial_pose_y",tmpList,tmpString);
//    if(1==tmpResult)
//    {
//        initial_pose_y=tmpString.toDouble();
//    }
//    else
//    {
//        return -16;
//    }
//    tmpResult=getValue("initial_pose_a",tmpList,tmpString);
//    if(1==tmpResult)
//    {
//        initial_pose_a=tmpString.toDouble();
//    }
//    else
//    {
//        return -17;
//    }

    tmpResult=getValue("initial_cov_x",tmpList,tmpString);
    if(1==tmpResult)
    {
        initial_cov_x=tmpString.toDouble();
    }
    else
    {
        return -17;
    }

    tmpResult=getValue("initial_cov_y",tmpList,tmpString);
    if(1==tmpResult)
    {
        initial_cov_y=tmpString.toDouble();
    }
    else
    {
        return -17;
    }

    tmpResult=getValue("initial_cov_r",tmpList,tmpString);
    if(1==tmpResult)
    {
        initial_cov_r=tmpString.toDouble();
    }
    else
    {
        return -17;
    }

    tmpResult=getValue("badMatchScore",tmpList,tmpString);
    if(1==tmpResult)
    {
        badMatchScore=tmpString.toDouble();
    }
    else
    {
        return -17;
    }

    tmpResult=getValue("goodMatchScore",tmpList,tmpString);
    if(1==tmpResult)
    {
        goodMatchScore=tmpString.toDouble();
    }
    else
    {
        return -17;
    }


    tmpResult=getValue("newOdom_alpha_badMatchScore",tmpList,tmpString);
    if(1==tmpResult)
    {
        newOdom_alpha_badMatchScore=tmpString.toDouble();
    }
    else
    {
        return -17;
    }

    tmpResult=getValue("recovery_alpha_slow",tmpList,tmpString);
    if(1==tmpResult)
    {
        recovery_alpha_slow=tmpString.toDouble();
    }
    else
    {
        return -18;
    }

    tmpResult=getValue("recovery_alpha_fast",tmpList,tmpString);
    if(1==tmpResult)
    {
        recovery_alpha_fast=tmpString.toDouble();
    }
    else
    {
        return -19;
    }

    tmpResult=getValue("resample_interval",tmpList,tmpString);
    if(1==tmpResult)
    {
        resample_interval=tmpString.toDouble();
    }
    else
    {
        return -20;
    }

    tmpResult=getValue("odomOffsetFilterCount",tmpList,tmpString);
    if(1==tmpResult)
    {
        odomOffsetFilterCount=tmpString.toInt();
    }
    else
    {
        return -21;
    }

    tmpResult=getValue("isCorrectByReflect",tmpList,tmpString);
    if(1==tmpResult)
    {
        isCorrectByReflect=tmpString.toInt();
    }
    else
    {
        return -22;
    }

    tmpResult=getValue("qualifyReflectMinCertaintyScore",tmpList,tmpString);
    if(1==tmpResult)
    {
        qualifyReflectMinCertaintyScore=tmpString.toDouble();
    }
    else
    {
        return -22;
    }

    tmpResult=getValue("correctFilterCount",tmpList,tmpString);
    if(1==tmpResult)
    {
        correctFilterCount=tmpString.toInt();
    }
    else
    {
        return -23;
    }


    tmpResult=getValue("correctAllowMaxDistanceOffset",tmpList,tmpString);
    if(1==tmpResult)
    {
        correctAllowMaxDistanceOffset=tmpString.toDouble();
    }
    else
    {
        return -23;
    }


    tmpResult=getValue("correctAllowMaxAngleOffset",tmpList,tmpString);
    if(1==tmpResult)
    {
        correctAllowMaxAngleOffset=tmpString.toDouble();
    }
    else
    {
        return -24;
    }

    xmlParser.closeXml( );

    return 1;

}

int AmclConfig::writeAmclPara(AmclDebugPara paraIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFile, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, AmclConfig::writeConfigFile error！";
       return -1;
    }


    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "node", ok );
    if( !ok )
    {

        qDebug()<<"error, AmclConfig::configFile node error！";
        return -1;
    }

    QDomNodeList devNodes = inNode.childNodes();
    QDomNode devND;
    for(int i=0;i<devNodes.size();i++)
    {
        devND =  devNodes.at(i);
        if("param"==devND.nodeName())
        {
            if(true==devND.toElement().hasAttribute("name" ))
            {
                QString tmpNameValue= devND.toElement().attribute("name" );
                if("calculateTimesForLaser"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.calculateTimesForLaser);
                }
                else if("laser_max_beams"==tmpNameValue)
                {
                   devND.toElement().setAttribute("value",paraIn.laser_max_beams);
                }
                else if("min_particles"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.min_particles);
                }
                else if("max_particles"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.max_particles);
                }
                else if("update_min_d"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.update_min_d);
                }
                else if("update_min_a"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.update_min_a);
                }
                else if("odom_alpha1"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.odom_alpha1);
                }
                else if("odom_alpha2"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.odom_alpha2);
                }
                else if("odom_alpha3"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.odom_alpha3);
                }
                else if("odom_alpha4"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.odom_alpha4);
                }
                else if("odom_alpha5"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.odom_alpha5);
                }
                else if("goodMatchScore"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.goodMatchScore);
                }
                else if("badMatchScore"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.badMatchScore);
                }
                else if("newOdom_alpha_badMatchScore"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.newOdom_alpha_badMatchScore);
                }
                else if("recovery_alpha_slow"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.recovery_alpha_slow);
                }
                else if("recovery_alpha_fast"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.recovery_alpha_fast);
                }
                else if("resample_interval"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.resample_interval);
                }
                else if("odomOffsetFilterCount"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.odomOffsetFilterCount);
                }
                else if("isCorrectByReflect"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.isCorrectByReflect);
                }
                else if("qualifyReflectMinCertaintyScore"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.qualifyReflectMinCertaintyScore);
                }
                else if("correctFilterCount"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.correctFilterCount);
                }
                else if("correctAllowMaxDistanceOffset"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.correctAllowMaxDistanceOffset);
                }
                else if("correctAllowMaxAngleOffset"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",paraIn.correctAllowMaxAngleOffset);
                }
            }
        }


    }



    xmlParser.closeXml( true );

    return 1;
}

int AmclConfig::writeConfigFile(double initialPose_x, double initialPose_y, double initialPose_r)
{
    return writeConfigFile(configFile,initialPose_x,initialPose_y,initialPose_r);
}

int AmclConfig::writeConfigFile(QString filePath, double initialPose_x, double initialPose_y, double initialPose_r)
{
//    initialPose_x=-0.111;
//    initialPose_y=2.222;
//    initialPose_r=3.222;
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePath, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, AmclConfig::writeConfigFile error！";
       return -1;
    }


    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "node", ok );
    if( !ok )
    {

        qDebug()<<"error, AmclConfig::readConfigFile node error！";
        return -1;
    }

    QDomNodeList devNodes = inNode.childNodes();
    QDomNode devND;
    for(int i=0;i<devNodes.size();i++)
    {
        devND =  devNodes.at(i);
        if("param"==devND.nodeName())
        {
            if(true==devND.toElement().hasAttribute("name" ))
            {
                QString tmpNameValue= devND.toElement().attribute("name" );
                if("initial_pose_x"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",initialPose_x);
                }
                else if("initial_pose_y"==tmpNameValue)
                {
                   devND.toElement().setAttribute("value",initialPose_y);
                }
                else if("initial_pose_a"==tmpNameValue)
                {
                    devND.toElement().setAttribute("value",initialPose_r);
                }
            }
        }


    }



    xmlParser.closeXml( true );

    return 1;
}

int AmclConfig::getValue(QString nameIn, const QVector<RosXml> &infoList, QString &valueOut)
{
    for(int i=0;i<infoList.size();i++)
    {
        if(nameIn==infoList[i].name)
        {
            valueOut=infoList[i].value;
            return 1;
        }
    }
    return -1;
}
